/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"

#include <sstream>
#include <string.h>
#include "SerialPort_write.h"   /* ERROR Number Definitions           */
#include "xun_fei_angle_check/xunfei.h"

xun_fei_angle_check::xunfei ang;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "xf_wakeup");

  ros::NodeHandle n;


    ros::Publisher cmd_pub = n.advertise<xun_fei_angle_check::xunfei>("xunfei/angle_check", 1000);

    ros::Rate loop_rate(10);
    open_serial();

    char end_ok=0;
    int check_coming=0;

    int sernum=0;
    char angllee[4];

    while (ros::ok())
    {
      check_coming=1;
      sernum=0;

      while(check_coming && ros::ok())
      {
          read_data(&end_ok);
          switch(sernum)
          {
          case 0:if(end_ok=='a')sernum=1;break;
          case 1:if(end_ok=='n')sernum=2;else if(end_ok=='a')sernum=1;else sernum=0;break;
          case 2:if(end_ok=='g')sernum=3;else if(end_ok=='a')sernum=1;else sernum=0;break;
          case 3:if(end_ok=='l')sernum=4;else if(end_ok=='a')sernum=1;else sernum=0;break;
          case 4:if(end_ok=='e')sernum=5;else if(end_ok=='a')sernum=1;else sernum=0;break;
          case 5:if(end_ok==':')check_coming=0;else if(end_ok=='a')sernum=1;else sernum=0;break;
          }
      }

      if(ros::ok())
      {
          memset(angllee,0,4);
          for(sernum=0;end_ok !=10 && end_ok !=13 && sernum<4;sernum++)
          {
              read_data(&end_ok);
              angllee[sernum]=end_ok;
          }

          angllee[sernum-1]=0;
          check_coming=atoi(angllee);

          ang.angle=check_coming;

          cmd_pub.publish(ang);

          ros::spinOnce();

          loop_rate.sleep();
      }
    }
    close_serial();
    return 0;
}
